#ifndef TROYPOSE_H
#define TROYPOSE_H

#include <QSharedPointer>
#include <QVector3D>
#include <QGenericMatrix>
#include <QMatrix4x4>
#include <cfloat>

#include "robotpose.h"
#include "userskeleton.h"

/**
  * An extended implementation of the RobotPose class, designed to ease storing all metadata about a single Troy pose.
  * In addition to storing the joint angles (the primary metadata about a pose), TroyPose provides these features:
  *  * Processing of a Kinect keyframe (UserSkeleton) and retargetting it to Troy
  *  * Caching of the 'raw' (unsnapped and unfiltered) version of the retargetting
  *  * Caching of the Kinect UserSkeleton data
  *  * Forward kinematics for arms, shoulders, and several other points in Troy-space (needs testing, some points not
  *     yet implemented)
  *  * Storage of an expression to associate with the pose (as a QString reference to an image file)
  */
class TroyPose : public RobotPose {
public:
    // Definition of the joint channels Troy uses
    enum TroyJointChannel {
        LeftShoulderForward = 0,
        LeftShoulderOut = 1,
        LeftTwist = 8,
        LeftElbow = 9,
        HeadRotation = 12,
        RightShoulderForward = 16,
        RightShoulderOut = 17,
        RightTwist = 24,
        RightElbow = 25,
        HeadPitch = 28
    };
    // Definition of the points Troy can locate via forward kinematics
    enum TroyPoint {
        LeftShoulderPoint,
        LeftElbowPoint,
        LeftHandPoint,
        RightShoulderPoint,
        RightElbowPoint,
        RightHandPoint,
        BodyPoint,
        NeckPoint
    };
public:
    /**
      * Creates a default TroyPose, without any joints or expression.
      */
    TroyPose();
    /**
      * Creates a TroyPose, and retargets the given UserSkeleton to Troy. Also, it caches the intermediate data
      * to allow future analysis and reprocessing.
      */
    TroyPose(QSharedPointer<UserSkeleton> skeleton);
    /**
      * Creates a TroyPose, and retargets the given UserSkeleton to Troy, using hints as a reference to the previous frame
      * when snapping.
      * Also, it caches the intermediate data to allow future analysis and reprocessing.
      */
    TroyPose(QSharedPointer<UserSkeleton> skeleton, QMap<int, float> hints);
    /**
      * Destroys a TroyPose and deallocates memory associated with it.
      */
    ~TroyPose();

    /**
      * Sets the angle associated with a given joint channel for this pose.
      * Also clears the position cache for joints that channel affects so forward kinematics remain correct.
      *
      * @param channel
      * @param theta
      */
    virtual void setJointAngle(int channel, float theta);

    /**
      * Checks whether or not an expression has been given to this pose.
      */
    bool hasExpression() const;
    /**
      * Gets the expression associated with this pose.
      */
    QString expression() const;
    /**
      * Sets the expression associated with this pose.
      *
      * @param expr
      */
    void setExpression(QString expr);

    /**
      * Checks whether the indicated TroyPoint can be calculated with the current joint channels.
      * Note that since all forward kinematics are done using joint angles, those are what this method checks.
      *
      * @param joint
      */
    bool hasJointPosition(TroyPoint joint) const;
    /**
      * Gets the point in Troy-space for the indicated TroyPoint, by applying forward kinematics along joint angles.
      *
      * @param joint
      */
    QVector3D jointPosition(TroyPoint joint) const;

    /**
      * Checks whether this pose has an associated 'raw' pose that didn't have Troy's joint restraints applied to it.
      * All TroyPoses generated from a UserSkeleton will have one, though it may be identical to this.
      */
    bool hasRawPose() const;
    /**
      * Gets the 'raw' pose associated with this pose.
      */
    QSharedPointer<TroyPose const> rawPose() const;

    /**
      * Checks whether this pose has an associated UserSkeleton (a frame of data from the Kinect OpenNI framework).
      * This will happen if the TroyPose was instanciated from a UserSkeleton in its constructor.
      */
    bool hasUserSkeleton() const;
    /**
      * Gets the UserSkeleton associated with this pose.
      */
    QSharedPointer<UserSkeleton const> userSkeleton() const;

private:
    /**
      * Swaps the given TroyPose in as the raw pose for this pose.
      * In an ideal world, this would apply the snapping and retargetting fixes, but the way the current algorithm works
      * relies on the vector data that is generated in the process of retargetting to snap. So this just copies it right over.
      *
      * @param pose
      */
    //void setRawSkeleton(QSharedPointer<TroyPose> pose);
    /**
      * Sets the UserSkeleton associated with this pose, retargets it to Troy, and then sets the affected joints.
      * All is done using an empty hints list.
      *
      * The restrain parameter indicates whether or not the joint angles should be restrained to Troy's constraints.
      *
      * @param skeleton
      * @param restrain
      */
    void setUserSkeleton(QSharedPointer<UserSkeleton> skeleton, bool restrain = true);
    // Adds the ability to send hints - the jointMap of the previously retargetted pose
    // These allow for some padding around 180/-180 borders, so that the arm doesn't jump back and forth until it really
    // needs to.
    /**
      * Sets the UserSkeleton associated with this pose, retargets it, and then sets the affected joints. It also stores the
      * raw pose containing the pose angles before they were snapped.
      *
      * Hints is a list of channel => angle pairs that is referenced when snapping is required on joints whose retargetted
      * position is outside Troys range of motion, or when an angle rotates across the 180/-180 boundary. In order to prevent
      * Troy from simply oscillating rapidly between -180 and 180 degrees, by remembering the previous frame, it can snap
      * the joint to the same extreme it was on previously, at least until the difference is extreme enough that it must jump
      * back.
      *
      * If the hint for a specific joint is not in the map, it will simply snap to whichever side is closer.
      *
      * The restrain parameter indicates whether or not the joint angles should be restrained to Troy's constraints.
      *
      * @param skeleton
      * @param hints
      * @param restrain
      */
    void setUserSkeleton(QSharedPointer<UserSkeleton> skeleton, QMap<int, float> hints, bool restrain = true);

    // If allowSnapping is true, it will snap if required.
    // Returns whether or not it actually snapped anything. (e.g., if allowSnapping == false, it will always return false.)
    static bool buildUpperArmVectors(
        QSharedPointer<UserSkeleton> skeleton,
        UserSkeleton::UserJoint shoulderJoint,
        UserSkeleton::UserJoint elbowJoint,
        UserSkeleton::UserJoint handJoint,
        const QVector3D &backbone,
        const QVector3D &out,
        const QVector3D &forward,
        const QVector3D &snapCheck,
        // Output vectors! These are modified by the function!
        QVector3D &shoulder,
        QVector3D &upperArm,
        QVector3D &shoulderTwistVector,
        bool allowSnapping = false,
        float shoulderForwardHint = FLT_MAX
        );

    // Same thing with snapping as above
    static void buildLowerArmVectors(
        QSharedPointer<UserSkeleton> skeleton,
        UserSkeleton::UserJoint elbowJoint,
        UserSkeleton::UserJoint handJoint,
        const QVector3D &out,
        const QVector3D &upperArm,
        const QVector3D &shoulderTwistVector,
        //float shoulderOut, // This value is needed to invert the arm in certain quadrants, but will be recalculated
        // Output vectors! These are modified by the function!
        QVector3D &lowerArm,
        QVector3D &lowerSignVector,
        QVector3D &lowerStart,
        QVector3D &lowerTwistVector
        );

    static float finalizeJointAngle(const QVector3D& vector, const QVector3D& signComparator, const QVector3D& angleComparator);

    void clearCache(int channel);
    bool canMakePosition(TroyPoint joint) const;
    QVector3D makePosition(TroyPoint joint) const;

    bool hasLeftArm() const;
    bool hasRightArm() const;

private:
    // Utility functions for retargetting
    static QVector3D project(const QVector3D& vector, const QVector3D& axis);
    static QVector3D project(const QVector3D& vector, const QVector3D& axis1, const QVector3D& axis2);
    static qreal angleDifference(const QVector3D& vector1, const QVector3D& vector2);

private:
    QString expressionPath;
    mutable QMap<TroyPoint, QVector3D> positions;

    QSharedPointer<UserSkeleton> user;
    bool needsRaw;
    mutable QSharedPointer<TroyPose> raw;


    static const float jointRadius;
    static const float upperArmLength;
    static const float lowerArmLength;
};

#endif // TROYPOSE_H
